#!coding:utf-8
import numpy as np
import serial.tools.list_ports
from pi_driver import Servo, SServo


class ServoParams:
    def __init__(self):
        # 舵机旋转角度的中间值
        self.neutral_position = 1023/2

        # 理论计算方向与舵机实际转动方向不一定能完全一致
        # 由于安装位置不同需要对部分位置的舵机反向
        self.servo_multipliers = np.array(
            [[1, 1, 1, 1], [-1, 1, -1, 1], [1, -1, 1, -1]]
        )


class HardwareInterface:
    def __init__(self):
        # 查找串口设备
        serial_ports = [i[0] for i in serial.tools.list_ports.comports()]
        print(serial_ports)
        # 默认使用找到的第一个串口设备
        self.pi = SServo(serial_ports[0])
        self.speed = 2000
        self.servo_params = ServoParams()
        # 储存舵机角度
        self.servo_angles = [0 for i in range(12)]

    def set_actuator_postions(self, joint_angles):
        # joint_angles: 3x4数组 弧度制
        angles = []
        ids = []
        # 理论计算以水平和竖直方向为0°, 实际较准以45°为基准
        servo_offset = [0, 45, -45]
        for leg_index in range(4):
            for axis_index in range(3):
                # 计算各个关节对应的舵机编号
                id = leg_index*3+axis_index+1
                ids.append(id)
                # 弧度制转为角度制
                angle = -(joint_angles[axis_index][leg_index]/np.pi*180 - servo_offset[axis_index]
                          ) * self.servo_params.servo_multipliers[axis_index, leg_index]
                self.servo_angles[id-1] = int(angle)

                angles.append(int(angle))
                # print(angle)
        # 实际的零点位置
        servos = [Servo(ids[i], int(angles[i]/300.0*1023+self.servo_params.neutral_position), speed=self.speed)
                  for i in range(12)]
        # 发送舵机控制指令
        self.pi.set_positions_sync(servos)
        return ids, angles

    def set_actuator_angles(self, angles):
        # angles: 1x12数组 角度制
        ids = []
        for leg_index in range(4):
            for axis_index in range(3):
                id = leg_index*3+axis_index+1
                ids.append(id)
        servos = [Servo(ids[i], int(angles[i]/300.0*1023+self.servo_params.neutral_position), speed=self.speed)
                  for i in range(12)]
        self.pi.set_positions_sync(servos)

    def slow_up(self):
        servos = [Servo(i+1, int(self.servo_params.neutral_position), ms=2000)
                  for i in range(12)]
        # 发送舵机控制指令
        self.pi.set_positions_sync(servos)

if __name__ == '__main__':
    hardwareInterface = HardwareInterface()
